技术文档｜一文了解 Apollo RTK定位流程 |
您所在的位置：网站首页 › rtk 导航 › 技术文档｜一文了解 Apollo RTK定位流程 |
|
从自动驾驶技术出发，Apollo开发者社区带你一起探索自动驾驶开发的奥秘，给每一位对自动驾驶充满热爱的你带来最实质的帮助，助力你的每一次研发。本文将介绍 Apollo RTK定位流程，主要从以下几点为大家进行详细讲解： 概览Localization模块主要实现了以下两个功能： 输出车辆的位置信息（Planning模块使用）输出车辆的姿态，速度信息（Control模块使用）其中Apollo代码中分别实现了三种定位方法： GNSS + IMU定位NDT定位（点云定位）MSF（融合定位）MSF 方法参考论文“Robust and Precise Vehicle Localization based on Multi-Sensor Fusion in Diverse City Scenes” 目录代码下面是Localization的目录结构，在查看具体的代码之前最好看下定位模块的Readme文件: ├── common // 声明配置(flags)，从conf目录中读取相应的值├── conf // 配置文件存放目录├── dag // cyber DAG流├── launch // cyber的配置文件，依赖DAG图（这2个和cyber有关的后面再分析）├── msf // 融合定位（gnss,点云,IMU融合定位）│ ├── common│ │ ├── io│ │ ├── test_data│ │ └── util│ ├── local_integ│ ├── local_map│ │ ├── base_map│ │ ├── lossless_map│ │ ├── lossy_map│ │ ├── ndt_map│ │ └── test_data│ ├── local_tool│ │ ├── data_extraction│ │ ├── local_visualization│ │ └── map_creation│ └── params│ ├── gnss_params│ ├── vehicle_params│ └── velodyne_params├── ndt // ndt定位│ ├── map_creation│ ├── ndt_locator│ └── test_data│ ├── ndt_map│ └── pcds├── proto // 消息格式├── rtk // rtk定位└── testdata // imu和gps的测试数据通过上述目录可以知道，定位模块主要实现了RTK，NDT，MSF这三个定位方法，分别对应不同的目录。Proto文件夹定义了消息的格式，Common和Conf主要是存放一些配置和消息TOPIC。下面逐个分析RTK定位、NDT定位和MSF定位。 RTK定位流程RTK定位是通过GPS和IMU的信息做融合然后输出车辆所在的位置。RTK通过基准站的获取当前GPS信号的误差，用来校正无人车当前的位置，可以得到厘米级别的精度。IMU的输出频率高，可以在GPS没有刷新的情况下（通常是1s刷新一次）用IMU获取车辆的位置。下面是RTK模块的目录结构： ├── BUILD // bazel编译文件├── rtk_localization.cc // rtk定位功能实现模块├── rtk_localization_component.cc // rtk消息发布模块├── rtk_localization_component.h├── rtk_localization.h└── rtk_localization_test.cc // 测试其中"rtk_localization_component.cc"注册为标准的Cyber模块，RTK定位模块在"Init"中初始化，每当接收到"localization::Gps"消息就触发执行"Proc"函数。 class RTKLocalizationComponent final : public cyber::Component { public: RTKLocalizationComponent(); ~RTKLocalizationComponent() = default; bool Init() override; bool Proc(const std::shared_ptr &gps_msg) override;下面分别查看这两个函数： Init函数Init函数实现比较简单，一是初始化配置信息，二是初始化IO。初始化配置信息主要是读取一些配置，例如一些Topic信息等。下面主要看下初始化IO： bool RTKLocalizationComponent::InitIO() { // 1.读取IMU信息，每次接收到localization::CorrectedImu消息，则回调执行“RTKLocalization::ImuCallback” corrected_imu_listener_ = node_->CreateReader( imu_topic_, std::bind(&RTKLocalization::ImuCallback, localization_.get(), std::placeholders::_1)); CHECK(corrected_imu_listener_); // 2.读取GPS状态信息，每次接收到GPS状态消息，则回调执行"RTKLocalization::GpsStatusCallback" gps_status_listener_ = node_->CreateReader( gps_status_topic_, std::bind(&RTKLocalization::GpsStatusCallback, localization_.get(), std::placeholders::_1)); CHECK(gps_status_listener_); // 3.发布位置信息和位置状态信息 localization_talker_ = node_->CreateWriter(localization_topic_); CHECK(localization_talker_); localization_status_talker_ = node_->CreateWriter(localization_status_topic_); CHECK(localization_status_talker_); return true;}也就是说，RTK模块同时还接收IMU和GPS的状态信息，然后触发对应的回调函数。具体的实现在"RTK Localization"类中，先看下回调的具体实现。以"Gps StatusCallback"为例，每次读取到GPS状态信息之后，会把信息保存到"gps_status_list_"列表中。"IMU Callback"类似，也是接收到IMU消息后，保存到"imu_list_"列表中。 void RTKLocalization::GpsStatusCallback( const std::shared_ptr &status_msg) { std::unique_lock lock(gps_status_list_mutex_); if (gps_status_list_.size() < gps_status_list_max_size_) { gps_status_list_.push_back(*status_msg); } else { gps_status_list_.pop_front(); gps_status_list_.push_back(*status_msg); }}Proc在每次接收到"localization::Gps"消息后，触发执行"Proc"函数。这里注意如果需要接收多个消息，这里是三个消息，则选择最慢的消息作为触发，否则，如果选择比较快的消息作为触发，这样会导致作为触发的消息刷新了，而其它的消息还没有刷新。所以这里采用的是GPS消息作为触发消息，IMU的消息刷新快。下面看具体的实现： bool RTKLocalizationComponent::Proc( const std::shared_ptr& gps_msg) { // 1. 通过RTKLocalization处理GPS消息回调 localization_->GpsCallback(gps_msg); if (localization_->IsServiceStarted()) { LocalizationEstimate localization; // 2. 获取定位消息 localization_->GetLocalization(&localization); LocalizationStatus localization_status; // 3. 获取定位状态 localization_->GetLocalizationStatus(&localization_status); // publish localization messages // 4. 发布位置信息 PublishPoseBroadcastTopic(localization); // 5. 发布位置转换信息 PublishPoseBroadcastTF(localization); // 6. 发布位置状态信息 PublishLocalizationStatus(localization_status); ADEBUG std::numeric_limits::min()) { break; } } if (imu_it != imu_list.end()) { // found one if (imu_it == imu_list.begin()) { AERROR set_x(vec[0]); mutable_pose->mutable_linear_acceleration()->set_y(vec[1]); mutable_pose->mutable_linear_acceleration()->set_z(vec[2]); // linear_acceleration_vfr // 设置线性加速度 mutable_pose->mutable_linear_acceleration_vrf()->CopyFrom( imu.linear_acceleration()); } else { AERROR CopyFrom(imu.euler_angles()); } }}Find Nearest GPS StatusFind Nearest GPS StatusFind Nearest GPS StatusFind Nearest GPS StatusFind Nearest GPS Status 获取最近的GPS状态信息，这里实现的算法是遍历查找离"gps_time_stamp"最近的状态，GPS状态信息不是按照时间顺序排列的？ bool RTKLocalization::FindNearestGpsStatus(const double gps_timestamp_sec, drivers::gnss::InsStat *status) { ... // 1. 遍历查找最近的GPS状态信息 double timestamp_diff_sec = 1e8; auto nearest_itr = gps_status_list.end(); for (auto itr = gps_status_list.begin(); itr != gps_status_list.end(); ++itr) { double diff = std::abs(itr->header().timestamp_sec() - gps_timestamp_sec); if (diff < timestamp_diff_sec) { timestamp_diff_sec = diff; nearest_itr = itr; } } ...}Fill Localization Status MsgFill Localization Status Msg 获取位置状态，一共有三种状态：稳定状态(INS_RTKFIXED)、浮动状态(INS_RTKFLOAT)、错误状态(ERROR)。由于代码比较简单，这里就不分析了。 发布消息 最后通过以下几个函数发布消息。 PublishPoseBroadcastTopic // 发布位置信息PublishPoseBroadcastTF // 发布位置转换transform信息PublishLocalizationStatus // 发布位置状态信息以上就是整个RTK的定位流程，主要的思路是通过接收GPS和IMU信息结合输出无人车的位置信息。 |
今日新闻 |
推荐新闻 |
| CopyRight 2018-2019 办公设备维修网 版权所有 豫ICP备15022753号-3 |